#include "includes/FeatureExtraction.h"

void getKeypoint(cv::Mat _Image, std::vector<cv::KeyPoint> *_Keypoints)
{
    cv::SiftFeatureDetector *detector = new cv::SiftFeatureDetector(100, 3, 0.04, 10, 1.6);
    detector->detect(_Image, *_Keypoints);
    detector->~DescriptorExtractor();
}

void getKeypoint(cv::Mat _Image, std::vector<cv::KeyPoint> *_Keypoints, int _numFeatures)
{
    cv::SiftFeatureDetector *detector = new cv::SiftFeatureDetector(_numFeatures, 3, 0.04, 10, 1.6);
    detector->detect(_Image, *_Keypoints);
    detector->~DescriptorExtractor();
}

void getDescriptors(cv::Mat _Image, std::vector<cv::KeyPoint> _Keypoints, cv::Mat *_Descriptors)
{
    cv::SiftDescriptorExtractor *extractor = new cv::SiftDescriptorExtractor();
    extractor->compute(_Image, _Keypoints, *_Descriptors);
    extractor->~DescriptorExtractor();
}

void getDescriptors(cv::Mat _Image, std::vector<cv::KeyPoint> *_Keypoints, cv::Mat *_Descriptors, int _numFeatures)
{
    std::vector<cv::KeyPoint> _ImageKeypoints;
    getKeypoint(_Image, &_ImageKeypoints, _numFeatures);

    cv::SiftDescriptorExtractor *extractor = new cv::SiftDescriptorExtractor();
    extractor->compute(_Image, _ImageKeypoints, *_Descriptors);

    extractor->~DescriptorExtractor();
    *_Keypoints = _ImageKeypoints;
}

void getMatches(cv::Mat _DescriptorsL, cv::Mat _DescriptorsR, std::vector<cv::DMatch> *_Matches)
{

    cv::FlannBasedMatcher *FeatureMatcher = new cv::FlannBasedMatcher();
    std::vector<std::vector<cv::DMatch> > Matches;
    std::vector<cv::DMatch> good_matches;

    FeatureMatcher->knnMatch(_DescriptorsL, _DescriptorsR, Matches, 2);

    const float ratio = 0.5; // As in Lowe's paper; can be tuned
    for (uint i = 0; i < Matches.size(); ++i)
    {
        if (Matches[i][0].distance < ratio * Matches[i][1].distance)
        {
            good_matches.push_back(Matches[i][0]);
        }
    }

    *_Matches = good_matches;

    FeatureMatcher->~DescriptorMatcher();
}

void getMatchedImage(cv::Mat _ImageL, cv::Mat _ImageR, std::vector<cv::KeyPoint> _KeypointsL, std::vector<cv::KeyPoint> _KeypointsR,
                     std::vector<cv::DMatch> _Matches, cv::Mat *_ImgMatches)
{
    cv::drawMatches(_ImageL, _KeypointsL, _ImageR, _KeypointsR, _Matches, *_ImgMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
                    //                     std::vector <std::vector<char> >(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
                    std::vector<char>());
    //        for( int i = 0; i < (int)this->nGoodMatches.size(); i++ )
    //        { printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, this->nGoodMatches[i].queryIdx, this->nGoodMatches[i].trainIdx ); }

}
